%Main Particle Filter method.
% initialLocation is [x, y, theta]
% particle_simulated(M,T,[5 5])
function finalLocation =  particle_simulated (map, target, initialLocation)
simulatedRobot = initialLocation;
particles = initialise(map);
plan = [0, 0];
req_weight = 0.95; %The required weighting of the largest particle to check for termination
%Main loop
while 1
    signal = getSimulatedReading(simulatedRobot, map);
    simulatedRobotOld = simulatedRobot;
    valid = 0;
    while valid == 0
        [simulatedRobot, plan(1)] = simulator_move(simulatedRobotOld, signal, plan(1));
        valid = inpolygon(simulatedRobot(1),simulatedRobot(2),map(:,1),map(:,2));
        if valid == 0
            plan = [rand(1), pi/2*rand(1) - pi/4];
        end
    end
    simulatedRobot = simulator_rotate(simulatedRobot, plan(2));
    
    particles = move_particles(particles, plan);
    
    %displaySimulatedRobot(simulatedRobot, map);
    displaySimulatedRobotAndParticles(simulatedRobot, particles, map);
    
    particle_signals = get_particle_measurements(particles, map);
    %Reweight returns a sorted list of particles, according to weights, biggest first.
    particles = reweight(particles, particle_signals, signal, map);
    disp(particles(1, :))
    %If the robot is lost reinitialise the map, and then continue
    %looping.
    if is_lost(particles(1, 4));
        disp('lost')
        particles = initialise(map);
        plan = [10*rand(1), pi/2*rand(1) - pi/4];
        %If the biggest particle's weight is greater than the threshhold,
        %and the robot is at the target point, stop looping.
    else if particles(1, 2) > req_weight && (particles(1, 1) == target(1)) && (particles(1, 2) == target(2))
            break
        end
        %plan = plan_motion(particles(1, 1), target);
        plan = [rand(1), pi/2*rand(1) - pi/4]; %random movement for testing
        %particles = normalise(particles);
        particles = resample(particles, map);
        if size(particles, 1) == 0
            particles = initialise(map);
        end
    end
end

finalLocation = simulatedRobot(1:2);
end
